#!/usr/bin/env python
# license removed for brevity

import rospy
import tf
import math


# Brings in the SimpleActionClient
import actionlib
# Brings in the .action file and messages used by the move base action
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal

goals = [[0.5,7],[0,21]]



def movebase_client(pos_x, pos_y):

   # Create an action client called "move_base" with action definition file "MoveBaseAction"
    client = actionlib.SimpleActionClient('move_base',MoveBaseAction)
 
   # Waits until the action server has started up and started listening for goals.
    client.wait_for_server()

   # Creates a new goal with the MoveBaseGoal constructor
    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id = "map"
    goal.target_pose.header.stamp = rospy.Time.now()
   # Move 0.5 meters forward along the x axis of the "map" coordinate frame 
    goal.target_pose.pose.position.x = pos_x
    goal.target_pose.pose.position.y = pos_y
   # No rotation of the mobile base frame w.r.t. map frame
    goal.target_pose.pose.orientation.w = 1.0

   # Sends the goal to the action server.
    client.send_goal(goal)
   # Waits for the server to finish performing the action.
   # wait = client.wait_for_result()
   # If the result doesn't arrive, assume the Server is not available
   # if not wait:
   #     rospy.logerr("Action server not available!")
   #     rospy.signal_shutdown("Action server not available!")
   # else:
    # Result of executing the action
    #    return client.get_result()   

# If the python node is executed as main process (sourced directly)
if __name__ == '__main__':
    # Initializes a rospy node to let the SimpleActionClient publish and subscribe
    rospy.init_node('set_goal_node')
    rate = rospy.Rate(10.0)
    print("start node")

    listener = tf.TransformListener()

    i  = 0
    pos_x = goals[i][0]
    pos_y = goals[i][1]
    movebase_client(pos_x, pos_y)

    
    while not rospy.is_shutdown():
        #print("in listen")
        try:
            (trans,rot) = listener.lookupTransform('/map', '/laser', rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue
        
        #print(trans[0],trans[1],trans[2])

        if trans[1] > 0.9* goals[i][1] and i < len(goals)-1:
            i = i+1
            pos_x = goals[i][0]
            pos_y = goals[i][1]
            movebase_client(pos_x, pos_y)
            #rospy.loginfo("Go to next goal!")
            print("Go to next goal!:", pos_x, pos_y)
            #i = i+1
        rate.sleep()
